/* Copyright(C): Leaf Johnson 2009,2010

    This file is part of leaf-lib.

    leaf-lib is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    leaf-lib is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with leaf-lib.  If not, see <http://www.gnu.org/licenses/>.


 * 
 * 
 */


/*                ****ROBOCON 2009 | BUPT TEAM*******
 * ------------------------------------------------------------------------
 * FileName   : cmtmsg.c
 * Version    : 1.0
 * Brief      : Mti消息相关的操作
 * Code by    : leaf
 * Date       : Apr 3, 2009
 * Note       : 消息的数据空间会自动增加，但是在消息不用时应该调用CmtMsgFreeData()
 *              释放数据空间，以防止内存泄漏
 *              向消息添加数据推荐使用appendData/appendByte方法，不要手动更改size
 *
 * ------------------------------------------------------------------------
 */
#include <devices_ext/cmtmsg.h>

/*初始化一条消息，datasize为初始分配的数据空间，推荐为2*/
void cmtmsg_init(CmtMsg *msg, size_t datasize){
	msg->size = 0;
	msg->mid  = 0;
	vectorInit(&msg->data, datasize);
}

/*设置消息ID，推荐使用这个函数而不是直接设置*/
void cmtmsg_setid(CmtMsg *msg, CmtMsgId id){
	msg->mid = id;
}

/*向消息中添加参数len指定长度的数据，自动改变size变量*/
void cmtmsg_append(CmtMsg *msg, const void *data, size_t len){
	msg->size += vectorWrite(&msg->data, msg->size, (const VectorDataType *)data, len);
}

/*向消息中添加一字节的数据*/
void cmtmsg_append_byte(CmtMsg *msg, uint8_t data){
	vectorSet(&msg->data, msg->size, data);
	if( msg->size < msg->data.capacity )
		++ msg->size;
}

/*复位size变量，相当于使当前消息体中的数据无效，但是不会释放数据空间*/
void cmtmsg_reset(CmtMsg *msg){
	msg->size = 0;
}

/*复位size变量，同时释放数据空间*/
void cmtmsg_free(CmtMsg *msg){
	msg->size = 0;
	vectorFree(&msg->data);
}


/*从消息中读取一字节的数据*/
uint8_t cmtmsg_byte(const CmtMsg *msg, size_t idx){
	return vectorGet(&msg->data, idx);
}

/* 从消息中提取一个浮点数，但是必需知道这个浮点数从数据区哪个字节开始
 * (如果只返回欧拉角度，则分别从0, 4, 8开始）
 * eg: pitch = cmtMsgGetFloat(msg, 4);
 * */
float cmtmsg_get_float(const CmtMsg *msg, size_t idx){
	float ret;
	uint8_t *dest = (uint8_t *)&ret;
	dest[0] = vectorGet(&msg->data, idx + 3);
	dest[1] = vectorGet(&msg->data, idx + 2);
	dest[2] = vectorGet(&msg->data, idx + 1);
	dest[3] = vectorGet(&msg->data, idx + 0);
	return ret;
}

/*
 *  当MTi返回数据以1220定点数形式时，可以用这个函数取得这个32位定点数的前16位
 * （1220的意思：前12位是符号位＋整数部分，后20位是小数部分）
 */
int16_t cmtmsg_get_int16(const CmtMsg *msg, size_t idx){
	int16_t ret;
	uint8_t *dest = (uint8_t *)&ret;
	dest[0] = vectorGet(&msg->data, idx + 1);
	dest[1] = vectorGet(&msg->data, idx);
	return ret;
}

/* 当MTi返回数据是1220格式时，取得其符号和整数部分*/
int16_t cmtmsg_get_angle(const CmtMsg *msg, size_t idx){
	int16_t raw = cmtmsg_get_int16(msg, idx);
	Angle ang = raw >> 4;
	ang += ( raw % 16 / 8 );
	return cmt_adjust_angle( ang );
}

/* 当MTi返回数据是1220格式时，转化为浮点格式*/
double cmtmsg_get_f1220(const CmtMsg *msg, size_t idx){
	double ret;
	int32_t tmp;
	uint8_t* dest = (uint8_t*) &tmp;
	dest[0] = vectorGet(&msg->data, idx + 3);
	dest[1] = vectorGet(&msg->data, idx + 2);
	dest[2] = vectorGet(&msg->data, idx + 1);
	dest[3] = vectorGet(&msg->data, idx + 0);

	ret = ((double) tmp)/1048576.0;
	return ret;
}

int be2le_16( const void *resbuf )
{
	int raw = 0;
	uint8_t *dest = (uint8_t *)&raw;
	const uint8_t *src = (const uint8_t *)resbuf;
	dest[0] = src[1];
	dest[1] = src[0];
	int ret = raw >> 4;
	ret += ( raw % 16 / 8 );
	return cmt_adjust_angle( ret );
}
/* 调整角度值，使它在[-180, 180)之间（佗螺仪返回的角度范围） */
Angle cmt_adjust_angle(Angle a){
	Angle ret = a;
	while( ret >= 180 )
		ret -= 360;
	while( ret < -180 )
		ret += 360;
	return ret;
}

/* 从佗螺仪返回的消息中提取角度信息(F1220格式) */
int cmt_msg_get_intgroup( pCmtMsg msg, size_t idx, pCmtIntGroup orient){
	orient->x = cmtmsg_get_angle( msg, idx );
	orient->y = cmtmsg_get_angle( msg, idx + 4 );
	orient->z = cmtmsg_get_angle( msg, idx + 8 );
	return 1;
}


#ifdef _STDIO_H_
/*显示一条消息，使用标准的输入/输出，作为调试中使用*/
void cmtMsgDisplay(CmtMsg *msg, FILE *dev){
	fprintf(dev, "I_%3u ", msg->mid);
	fprintf(dev, "S_%3u\n", msg->size);
	int i;
	fprintf(dev, "D_");
	if(msg->size <= 0){
		fprintf(dev, "N/A \n");
	}
	else{
		for(i = 0; i < msg->size; ++i){
			fprintf(dev, "%02x ", (uint8_t)vectorGet(&msg->data, i));
		}
		fprintf(dev, "\n");
	}
}
#endif
